#!/usr/bin/python
# coding=UTF-8
import os
import cv2
from detectors import SignDetector, td
import time
from lock import *
from camera import front_cam_Object
from camera import right_cam_Object, left_cam_Object
import multiprocessing
from cruise import logging
import numpy as np

image_extensions = [".png", ".jpg", ".jpeg"]


def draw_res(frame, results):
    """
     画锚框
    :param frame:
    :param results:
    :return:
    """
    res = list(frame.shape)
    # print(results)
    # print('aa')
    for item in results:
        left = item.relative_box[0] * res[1]
        top = item.relative_box[1] * res[0]
        right = item.relative_box[2] * res[1]
        bottom = item.relative_box[3] * res[0]
        start_point = (int(left), int(top))
        end_point = (int(right), int(bottom))
        color = (0, 244, 10)
        thickness = 2
        frame = cv2.rectangle(frame, start_point, end_point, color, thickness)
        font = cv2.FONT_HERSHEY_SIMPLEX
        org = start_point[0], start_point[1] - 10
        fontScale = 0.5
        frame = cv2.putText(frame, item.name + str((left + right) // 2) + ' ' + str((top + bottom) // 2), org, font,
                            fontScale, color, thickness, cv2.LINE_AA)
        return frame, int(left + right) // 2, int(top + bottom) // 2


class DetectModel:
    def __init__(self):
        STATUS_detect.value = True  # 用来表示地标检测的状态
        self.sign_process = None
        x_detect_model.value = 0  # 目标检测到的地标的x坐标
        y_detect_model.value = 0  # 目标检测到的地标的y坐标，现在换作共享变量
        RESULT_INDEX_detect.value = 0  # 结果的索引，0为背景

        self.first = True

    def test_sign(self):
        """
        前向车道地面标志动态识别
        :return:
        """
        time.sleep(1)
        sd = SignDetector()  # 实例化地标检测对象
        time.sleep(1)
        while True:
            if not STATUS_detect.value:
                pass
            else:
                try:  # 防止读到空图报错
                    a = time.time()
                    frame = front_cam_Object.read()
                    signs, index = sd.detect(frame)
                    logging.debug('##########detecting##########')
                    # TODO:查看背景下signs是什么东西
                    # cv2.imshow("a",frame)
                    # cv2.waitKey(1)
                    if signs != []:
                        RESULT_INDEX_detect.value = 0  # 还未到地标跟前先让index赋值为0
                        logging.debug("modeltest->signs!=[]->result_index{}".format(RESULT_INDEX_detect.value))
                        _, x_detect_model.value, y_detect_model.value = draw_res(frame, signs)
                        # cv2.imshow("a",frame)
                        # cv2.waitKey(1)
                    else:
                        # cv2.imshow("a",frame)
                        # cv2.waitKey(1)
                        RESULT_INDEX_detect.value = 0
                        logging.debug("modeltest->signs=[]->result_index{}".format(RESULT_INDEX_detect.value))

                    if 95 < x_detect_model.value < 240 and y_detect_model.value > 200:  # y=225时底盘前端距离地标5cm,y=205时6.5cm，y=210时4.6cm，y=215时4.3cm，y=220时3.1cmy=225时2.1cm TODO:这个阈值改好了！
                        RESULT_INDEX_detect.value = int(signs[0].index)
                        logging.debug("modeltest->we want result_index{}".format(RESULT_INDEX_detect.value))
                        if RESULT_INDEX_detect.value != 0:  # 如果目标检测的结果不是背景0，那么就退出这个循环 TODO:当满足有10个符合条件才判断成立
                            self.stop()

                    logging.debug("modeltest->test_sign_spend_time{}".format(time.time() - a))
                except Exception as e:
                    logging.debug('raise an error at test_sign in modeltest')
                    logging.error(e)
                    pass

    def start(self):
        """
        开启进程
        :return:
        """
        x_detect_model.value = 0  # 每次检测到，下次开启地标检测时需要赋值为0
        y_detect_model.value = 0  # ~的y坐标
        if self.first:
            STATUS_detect.value = True
            multiprocessing.Process(target=self.test_sign).start()
            self.first = False
        else:
            STATUS_detect.value = True

    def stop(self):
        """
        这是关闭线程吗
        :return:
        """
        STATUS_detect.value = False


class TaskModel:
    def __init__(self):
        self.x = 0  # 目标检测到的地标的x坐标
        self.y = 0  # 目标检测到的地标的x坐标的y坐标
        self.result_index = 0  # 结果的索引，0为背景

    def test_task(self, camera_name, fortress_resize):
        """
        侧面任务动态识别
        :return:
        """
        b = time.time()
        if camera_name == 1:
            side_image = right_cam_Object.read()
        else:
            side_image = left_cam_Object.read()
        if fortress_resize ==1:
            side_image = side_image[40:280,:]
        tasks = td.detect(side_image)
        # cv2.imshow("d",side_image)
        # cv2.waitKey(1)
        if tasks != []:
            _, self.x, self.y = draw_res(side_image, tasks)
            self.result_index = int(tasks[0].index)
            cv2.imshow("d",side_image)
            cv2.waitKey(1)
            logging.debug('*************self.x:{}'.format(self.x))
            logging.debug('##########drawing_task_res##########')
            logging.debug("modeltest->task_spend_time{}".format(time.time() - b))
            return True
        else:
            return False

def forage_detector(image, image_rows, image_cols):
    """
    用于检测放粮草/小球的那个框
    :param image: 传进来的图片
    :param image_rows: 图片的行数
    :param image_cols: 图片的列数
    :return:
    """
    imgHSV = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)  # 转换成HSV空间
    blurred = cv2.GaussianBlur(imgHSV, (5, 5), 0)  # 高斯模糊
    imgThresholded = cv2.inRange(blurred, np.array([156, 43, 46]), np.array([180, 255, 255]))  # 二值化

    # 形态学操作滤除噪点
    element = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
    imgdilate = cv2.dilate(imgThresholded, element)  # 膨胀
    imgMORPH = cv2.morphologyEx(imgdilate, cv2.MORPH_CLOSE, element)  # 先腐蚀后膨胀操作，原为腐蚀操作

    # 查找轮廓
    _, contours, hierarchy = cv2.findContours(imgMORPH, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    # contours, hierarchy = cv2.findContours(imgMORPH, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    # 找到最大的轮廓
    if len(contours):  # 如果轮廓数量不为0
        area = []
        for k in range(len(contours)):
            area.append(cv2.contourArea(contours[k]))
        max_idx = np.argmax(np.array(area))
        del area

        x, y, w, h = cv2.boundingRect(contours[max_idx])
        x1 = x  # 左上角坐标
        y1 = y
        x2 = x + w  # 右下角坐标
        y2 = y + h
        if x1 < 0:
            x1 = 0
        if x2 > image_cols:
            x2 = image_cols
        if y1 < 0:
            y1 = 0
        if y2 > image_rows:
            y2 = image_rows
        # cv2.rectangle(image, (x1, y1), (x2, y2), (225, 0, 0), thickness=2) # 用于观看框出的矩形
        # cv2.putText(image, str((x1 + x2) // 2) + ' ' + str((y1 + y2) // 2), (x1,y1), cv2.FONT_HERSHEY_SIMPLEX,
        #                     0.5, (255,0,0), 1, cv2.LINE_AA)
        # cv2.imshow("a",image)
        # cv2.waitKey(1)
        return (x1 + w // 2), (y1 + h // 2)  # 返回矩形中心点
    # 如果没有呢，应该返回什么


detect_Object = DetectModel()  # 实例化一个检测模型

task_Object = TaskModel()

if __name__ == "__main__":
    # os.system("stopx")
    # time.sleep(1)
    # os.system("startx")
    # time.sleep(1.5)
    right_cam_Object.start()
    # front_cam_Object.start()
    time.sleep(1)
    while True:
        try:
            task_Object.test_task(1,2)
        except Exception as e:
            logging.error(e)
    # detect_Object.test_sign()
